#pragma once
#include "Node.h"
#include "Grid.h"

template <typename T>
class AStar
{
	/* pseudocode taken from A* page on wikipedia

	function A*(start,goal)
    closedset := the empty set    // The set of nodes already evaluated.
    openset := {start}    // The set of tentative nodes to be evaluated, initially containing the start node
    came_from := the empty map    // The map of navigated nodes.

    g_score[start] := 0    // Cost from start along best known path.
    // Estimated total cost from start to goal through y.
    f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal)
     
    while openset is not empty
        current := the node in openset having the lowest f_score[] value
        if current = goal
            return reconstruct_path(came_from, goal)
         
        remove current from openset
        add current to closedset
        for each neighbor in neighbor_nodes(current)
            tentative_g_score := g_score[current] + dist_between(current,neighbor)
            tentative_f_score := tentative_g_score + heuristic_cost_estimate(neighbor, goal)
            if neighbor in closedset and tentative_f_score >= f_score[neighbor]
                    continue

            if neighbor not in openset or tentative_f_score < f_score[neighbor] 
                came_from[neighbor] := current
                g_score[neighbor] := tentative_g_score
                f_score[neighbor] := tentative_f_score
                if neighbor not in openset
                    add neighbor to openset

    return failure

	*/

public:
	//Constructor/destructor
	AStar(void);
	~AStar(void);

	//function that runs the algorithm
	vector<T*>* astar(T* startNode, T* goalNode, Grid* grid)
	{
		//open list and closed list
		vector<T*>* openList = new vector<T*>();
		vector<T*>* closedList = new vector<T*>();

		T* current;

		// Add starting node to open list
		openList.push_back(startNode);
		
		while (!openList.empty()) {
			current = getLowestNode();

			// Paul, I overloaded == in Node to check equivalency of location.
			if (current == goalNode)
				return reconstructPath(startNode, goalNode);

			// Remove current from openList (right now assuming it will always be at the front)
			openList->erase(openList->begin())
			// Add current to closedList
			closedList->push_back(current);

			// Iterate through neighbors
			vector<T*>* neighbors = grid->getNeighbors(current)

			for each (T* n : neighbors) {
				//If not in closed set and walkable=true, add to open set
				if(n->walkable == true && !std::find(neighbors->begin(), neighbors->end(), n))
				{
					//If the current node is not on the openList
					if(!std::find(neighbors->begin(), neighbors->end(), n))
					{
						//Put it on the openList
						openList->push_back(n);

						//find and record the h and g values of the node
						n->h = n->findH(goalNode);
						n->g = n->findG(startNode);

						//Set the current node as the parent of the neighbor
						n->parent = current;
					}
					//If the current node is already on the openList
					else
					{
						//Check to see if the new g value is better than the current g value
						//Determine what the current g value is
						int currentG = n->findG(startNode);
						
						//Save what the node's current parent node is
						T* currentParent = n->parent;

						//temporarily change the parent to the current node to calculate a new G vlaue
						n->parent = current;

						//find the potential new g value is
						int tempG = n->findG(startNode);

						//compare the new G value to the old g value.
						//If the old one was lower, revert to the previous parent node
						if(currentG < tempG)
						{
							n->parent = currentParent;

						}
						//If the new g value is lower, keep the parent of the new node as the current node and correct the g value
						else
						{
							n->g = tempG;
						}

					}
				}
			}
		}
	};

	T* getLowestNode() {
		int min = 10000;
		T* lowest = nullptr;
		
		for (T* n : openList)
			if (n->f < min) {
				lowest = n;
				min = n->f;
			}

		return lowest;
	}

	vector<T*>* reconstructPath(T* startNode, T*goalNode) {
		vector<T*>* path = new vector<T*>*();
		T* current = goalNode;

		path->push_back(goalNode);

		// Reconstructs the path by iteratively going backwards using the parent pointer.
		// The path is in backwards order.
		while (current != startNode) {
			path->push_back(current->parent);
			current = parent;
		}

		return path;
	}

};